//
// Created by LiuDongPeng on 2024/1/29.
//

#include "MT6701.h"



MT6701::MT6701(spi_send_recv_t spi_send_recv_cb, stm32_write_pin_t spi_set_cs_cb, bool cs_valid_level)
{
    m_spi_send_recv_cb = std::move(spi_send_recv_cb);
    m_spi_set_cs_cb = std::move(spi_set_cs_cb);
    m_cs_valid_level = cs_valid_level;
}

/**
 * @brief
 * @param encoderRaw
 * @param angleRad
 * @return
 */
std::tuple<bool, uint32_t, float> MT6701::update()
{
    int ret = 0;
    uint16_t tx = 0;
    uint16_t rx = 0;

    /**
     * [D13:D0]     [Mg3:Mg0]    [CRC5:CRC0]
     * 14 bit Data  4 bit Status 6 bit CRC
     */

    /* Reset CS pin, Start transform */
    m_spi_set_cs_cb(m_cs_valid_level);

    /* Recv date */
    ret = m_spi_send_recv_cb((uint8_t *) &tx, (uint8_t *) &rx, 1, 100);

    /* Set CS pin, Stop transform */
    m_spi_set_cs_cb(!m_cs_valid_level);

    /* Check read data frame */

    /* Get 14bit data */
    m_encoderRaw = rx >> 2;

    /* Calc angleRad */
    m_angleRad = (float)m_encoderRaw / (float)m_encoderCpr * (float)M_TWOPI;

    return std::make_tuple(ret == HAL_OK, m_encoderRaw, m_angleRad);
}
